#!/usr/bin/python
# coding=utf-8
import time
import rospy
import cv2 as cv
from geometry_msgs.msg import Twist
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Int8

class image_converter:
    def __init__(self):
        self.cmd_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)			# 发布运动控制信息
        self.moveclean_cmd = rospy.Publisher("/moveclean_cmd", Int8, queue_size=10)
        self.clean_cmd = rospy.Publisher("/clean_cmd", Int8, queue_size=10)
        self.bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, self.imageCallback)		# 订阅摄像头信息
        self.moveclean_report_sub = rospy.Subscriber("/moveclean_report", Int8, self.moveCleanReportCallback)
        self.clean_report_sub = rospy.Subscriber("/clean_report", Int8, self.cleanReportCallback)
        self.start_adjust_flag = True
        self.start_clean_flag = True
        self.finish_clean_flag = True
        self.direction_flag = True

    def imageCallback(self, data):
        try:
            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")			# 获取订阅的摄像头图像
        except CvBridgeError as e:
            print e
        # cv.imshow("Original_Image", cv_image)
        # 对图像进行处理
        kernel = cv.getStructuringElement(cv.MORPH_RECT, (3, 3))		# 定义结构元素
        height, width = cv_image.shape[0:2]
        screen_center = width / 2
        screen_center_h = height / 2
        
        offset = 50
        offset_h = 30
        lower_b = (35, 43, 46)
        upper_b = (77, 255, 255)
        
        hsv_frame = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV)				# 转成HSV颜色空间
        mask = cv.inRange(hsv_frame, lower_b, upper_b)
        if len(mask) > 0:								
            mask2 = cv.morphologyEx(mask, cv.MORPH_OPEN, kernel)			# 开运算去噪
            mask3 = cv.morphologyEx(mask2, cv.MORPH_CLOSE, kernel)			# 闭运算去噪
            # cv.imshow("mask", mask3)
            
            # 找出面积最大的区域
            _, contours, _ = cv.findContours(mask3, cv.RETR_EXTERNAL, cv.CHAIN_APPROX_SIMPLE)
            maxArea = 0
            maxIndex = 0
            for i, c in enumerate(contours):
                area = cv.contourArea(c)
                if area > maxArea:
                    maxArea = area
                    maxIndex = i
            # 绘制轮廓
            cv.drawContours(cv_image, contours, maxIndex, (255, 255, 0), 2)		

            # 获取外切矩形
            x, y, w, h = cv.boundingRect(contours[maxIndex])
            cv.rectangle(cv_image, (x, y), (x + w, y + h), (255, 0, 0), 2)
            # 获取中心像素点
            center_x = int(x + w / 2)
            center_y = int(y + h / 2)
            cv.circle(cv_image, (center_x, center_y), 5, (0, 0, 255), -1)
            # 显示图像
            # cv.imshow("Image", cv_image)

            if screen_center - offset <= center_x <= screen_center + offset:
                if self.start_adjust_flag:
                    moveclean_flag = Int8()          
                    moveclean_flag.data = 1
                    if (self.direction_flag):
                        moveclean_flag.data = 1
                    else: 
                        moveclean_flag.data = 2
                    i=0
                    while(1):
                        time.sleep(0.1)
                        self.moveclean_cmd.publish(moveclean_flag)
                        print "adjust start"
                        # self.start_adjust_flag = False
                        i+=1
                        if(i==1):
                            break
                # print "keep"
            

    #0 normal,1 get start ironing,2 get start cleaning, 3 finish cleaning, 
    #4 get force stop cleaning, 5 request stop moving, 6 request moving        
    def cleanReportCallback(self, msg):
        report_clean_flag = msg.data 
        if report_clean_flag == 2: #清洁板收到开始清洁命令
            self.start_clean_flag  = False
        elif report_clean_flag == 5: #清洁板请求停止运动
            twist = Twist()
            twist.linear.x = 0
            twist.angular.z = 0
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e
        elif report_clean_flag == 6: #清洁板请求运动
            twist = Twist()
            twist.linear.x = 0.02
            twist.angular.z = 0
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e
        elif report_clean_flag == 3: #清洁板上报清洁结束
            if self.finish_clean_flag:
                i=0
                while(1):
                    time.sleep(1) #刘鹤要求
                    clean_flag = Int8()
                    moveclean_flag = Int8()
                    clean_flag.data = 5 #3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    moveclean_flag.data = 5 # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    self.clean_cmd.publish(clean_flag) 
                    self.moveclean_cmd.publish(moveclean_flag) 
                    print "finish clean"
                    # self.finish_clean_flag = False
                    i+=1
                    if(i==1):
                        break

    def moveCleanReportCallback(self, msg):
        moveclean_report_status = msg.data
        if moveclean_report_status == 1:  #控制板收到开始对位
            self.start_adjust_flag = False
        elif moveclean_report_status == 2: #控制板发送对位结束，开始清洁
            if self.start_clean_flag:
                i=0
                while(1):
                    time.sleep(3) #刘鹤要求
                    moveclean_flag = Int8()
                    clean_flag = Int8()
                    moveclean_flag.data = 2 # start clean 0x27
                    clean_flag.data = 3 #
                    if (self.direction_flag):
                        moveclean_flag.data = 3
                        clean_flag.data = 3
                    else:
                        moveclean_flag.data = 4
                        clean_flag.data = 4
                    self.moveclean_cmd.publish(moveclean_flag) # 3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    self.clean_cmd.publish(clean_flag) #3 forward cleaning, 4 reverse cleaning, 5 stop cleaning
                    print "finish adjust and start clean"
                    i+=1
                    if(i==1):
                        break
        elif moveclean_report_status == 3: #控制板收到开始清洁
            self.start_clean_flag  = False
            twist = Twist()
            twist.linear.x = 0.02
            twist.linear.y = 0.0
            twist.angular.z = 0.0
            if (self.direction_flag):
                twist.linear.x = 0.02
            else:
                twist.linear.x = -0.02
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e
        elif moveclean_report_status == 4: #控制板收到清洁结束
            self.finish_clean_flag  = False
            twist = Twist()
            twist.linear.x = 0.0
            twist.linear.y = 0.0
            twist.angular.z = 0.0
            try:
                self.cmd_pub.publish(twist)
            except CvBridgeError as e:
                print e

if __name__ == '__main__':
    try:
        rospy.init_node("adjust_node")
        rospy.loginfo("Starting adjust node")
        image_converter()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down adjust node."
        cv.destroyAllWindows()
